
/**
  ******************************************************************************
  * Copyright 2021 The grapilot Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       mc_attitude_control.h
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdbool.h>
#include <stdint.h>

#include <pid/p.h>
#include <pid/pid.h>
#include <parameter/param.h>
#include <motor/gp_motors.h>
#include <common/gp_defines.h>
#include <common/gp_math/gp_mathlib.h>
/*-----------------------------------macro------------------------------------*/
#define AC_ATTITUDE_CONTROL_ANGLE_P                     4.5f             // default angle P gain for roll, pitch and yaw

#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MIN_RADSS       radians(40.0f)   // minimum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_RP_CONTROLLER_MAX_RADSS       radians(720.0f)  // maximum body-frame acceleration limit for the stability controller (for roll and pitch axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MIN_RADSS        radians(10.0f)   // minimum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS        radians(120.0f)  // maximum body-frame acceleration limit for the stability controller (for yaw axis)
#define AC_ATTITUDE_CONTROL_SLEW_YAW_DEFAULT_CDS        6000      // constraint on yaw angle error in degrees.  This should lead to maximum turn rate of 10deg/sec * Stab Rate P so by default will be 45deg/sec.
#define AC_ATTITUDE_CONTROL_ACCEL_RP_MAX_DEFAULT_CDSS   110000.0f // default maximum acceleration for roll/pitch axis in centidegrees/sec/sec
#define AC_ATTITUDE_CONTROL_ACCEL_Y_MAX_DEFAULT_CDSS    27000.0f  // default maximum acceleration for yaw axis in centidegrees/sec/sec

#define AC_ATTITUDE_RATE_CONTROLLER_TIMEOUT             1.0f    // body-frame rate controller timeout in seconds
#define AC_ATTITUDE_RATE_RP_CONTROLLER_OUT_MAX          1.0f    // body-frame rate controller maximum output (for roll-pitch axis)
#define AC_ATTITUDE_RATE_YAW_CONTROLLER_OUT_MAX         1.0f    // body-frame rate controller maximum output (for yaw axis)

#define AC_ATTITUDE_THRUST_ERROR_ANGLE                  radians(30.0f) // Thrust angle error above which yaw corrections are limited

#define AC_ATTITUDE_400HZ_DT                            0.0025f // delta time in seconds for 400hz update rate

#define AC_ATTITUDE_CONTROL_RATE_BF_FF_DEFAULT          1       // body-frame rate feedforward enabled by default

#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_TC_DEFAULT      1.0f    // Time constant used to limit lean angle so that vehicle does not lose altitude
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_THROTTLE_MAX    0.8f    // Max throttle used to limit lean angle so that vehicle does not lose altitude
#define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN             10.0f   // Min lean angle so that vehicle can maintain limited control

#define AC_ATTITUDE_CONTROL_MIN_DEFAULT                 0.1f    // minimum throttle mix default
#define AC_ATTITUDE_CONTROL_MAN_DEFAULT                 0.1f    // manual throttle mix default
#define AC_ATTITUDE_CONTROL_MAX_DEFAULT                 0.5f    // maximum throttle mix default
#define AC_ATTITUDE_CONTROL_MAX                         5.0f    // maximum throttle mix default

#define AC_ATTITUDE_CONTROL_THR_MIX_DEFAULT             0.5f  // ratio controlling the max throttle output during competing requests of low throttle from the pilot (or autopilot) and higher throttle for attitude control.  Higher favours Attitude over pilot input
/*----------------------------------typedef-----------------------------------*/
typedef struct
{
    Quat_t attitude_body;
    Vector3f_t gyro;
} AttCtrl_AttDataTypeDef;

typedef struct
{
    AttCtrl_AttDataTypeDef _att_data;

    float (*get_roll_trim_rad)(void);

    // Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes
    Param_float            _slew_yaw;

    // Maximum angular velocity (in degrees/second) for earth-frame roll, pitch and yaw axis
    Param_float            _ang_vel_roll_max;
    Param_float            _ang_vel_pitch_max;
    Param_float            _ang_vel_yaw_max;

    // Maximum rotation acceleration for earth-frame roll axis
    Param_float            _accel_roll_max;

    // Maximum rotation acceleration for earth-frame pitch axis
    Param_float            _accel_pitch_max;

    // Maximum rotation acceleration for earth-frame yaw axis
    Param_float            _accel_yaw_max;

    // Enable/Disable body frame rate feed forward
    Param_int8             _rate_bf_ff_enabled;

    // Enable/Disable angle boost
    Param_int8             _angle_boost_enabled;

    // Angle limit in centidegrees
    Param_int16            _angle_max;


    // angle controller P objects
    P_ctrl                _p_angle_roll;
    P_ctrl                _p_angle_pitch;
    P_ctrl                _p_angle_yaw;

    // Angle limit time constant (to maintain altitude)
    Param_float           _angle_limit_tc;

    // rate controller input smoothing time constant
    Param_float           _input_tc;

    // Intersampling period in seconds
    float                 _dt;

    // This represents a 321-intrinsic rotation in NED frame to the target (setpoint)
    // attitude used in the attitude controller, in radians.
    Vector3f_t            _euler_angle_target;

    // This represents the angular velocity of the target (setpoint) attitude used in
    // the attitude controller as 321-intrinsic euler angle derivatives, in radians per
    // second.
    Vector3f_t            _euler_rate_target;

    // This represents a quaternion rotation in NED frame to the target (setpoint)
    // attitude used in the attitude controller.
    Quat_t               _attitude_target;

    // This represents the angular velocity of the target (setpoint) attitude used in
    // the attitude controller as an angular velocity vector, in radians per second in
    // the target attitude frame.
    Vector3f_t            _ang_vel_target;

    // This represents the angular velocity in radians per second in the body frame, used in the angular
    // velocity controller.
    Vector3f_t            _ang_vel_body;

    // This is the the angular velocity in radians per second in the body frame, added to the output angular
    // attitude controller by the System Identification Mode.
    // It is reset to zero immediately after it is used.
    Vector3f_t            _sysid_ang_vel_body;

    // This is the the unitless value added to the output of the PID by the System Identification Mode.
    // It is reset to zero immediately after it is used.
    Vector3f_t            _actuator_sysid;

    // This represents a quaternion attitude error in the body frame, used for inertial frame reset handling.
    Quat_t              _attitude_ang_error;

    // The angle between the target thrust vector and the current thrust vector.
    float               _thrust_angle;

    // The angle between the target thrust vector and the current thrust vector.
    float               _thrust_error_angle;

    // throttle provided as input to attitude controller.  This does not include angle boost.
    float               _throttle_in;

    // This represents the throttle increase applied for tilt compensation.
    // Used only for logging.
    float               _angle_boost;

    // Specifies whether the attitude controller should use the square root controller in the attitude correction.
    // This is used during Autotune to ensure the P term is tuned without being influenced by the acceleration limit of the square root controller.
    bool                _use_sqrt_controller;

    // Filtered Alt_Hold lean angle max - used to limit lean angle when throttle is saturated using Alt_Hold
    float               _althold_lean_angle_max;

    // desired throttle_low_comp value, actual throttle_low_comp is slewed towards this value over 1~2 seconds
    float               _throttle_rpy_mix_desired;

    // mix between throttle and hover throttle for 0 to 1 and ratio above hover throttle for >1
    float               _throttle_rpy_mix;

    // Yaw feed forward percent to allow zero yaw actuator output during extreme roll and pitch corrections
    float               _feedforward_scalar;

    // References to external libraries
    Motors_HandleTypeDef* _motors;

    // true in inverted flight mode
    bool                _inverted_flight;
} Attitude_ctrl;

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
// 初始化姿态控制器
void attctrl_init(Attitude_ctrl * att_ctrl, Motors_HandleTypeDef* motors, float dt);

static inline void attctrl_set_dt(Attitude_ctrl * att_ctrl, float dt) { att_ctrl->_dt = dt; }

// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
void attctrl_input_euler_angle_roll_pitch_euler_rate_yaw(Attitude_ctrl * att_ctrl, float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);

// Command a thrust vector and heading rate
void attctrl_input_thrust_vector_rate_heading(Attitude_ctrl * att_ctrl, const Vector3f_t* thrust_vector, float heading_rate_cds);

// Command a thrust vector, heading and heading rate
void attctrl_input_thrust_vector_heading(Attitude_ctrl * att_ctrl, const Vector3f_t* thrust_vector, float heading_angle_cd, float heading_rate_cds);

Quat_t attctrl_attitude_from_thrust_vector(Vector3f_t thrust_vector, float heading_angle);

// Calculates the body frame angular velocities to follow the target attitude
void attctrl_attitude_controller_run_quat(Attitude_ctrl * att_ctrl);

// thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration.
void attctrl_thrust_heading_rotation_angles(Attitude_ctrl * att_ctrl, Quat_t* attitude_target, const Quat_t* attitude_body, Vector3f_t* attitude_error, float* thrust_angle, float* thrust_error_angle);

// thrust_vector_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion.
// The first rotation corrects the thrust vector and the second rotation corrects the heading vector.
void attctrl_thrust_vector_rotation_angles(const Quat_t* attitude_target, const Quat_t* attitude_body, Quat_t* thrust_vector_correction, Vector3f_t* attitude_error, float* thrust_angle, float* thrust_error_angle);

// Update rate_target_ang_vel using attitude_error_rot_vec_rad
Vector3f_t attctrl_update_ang_vel_target_from_att_error(Attitude_ctrl * att_ctrl, const Vector3f_t* attitude_error_rot_vec_rad);

// translates body frame acceleration limits to the euler axis
Vector3f_t attctrl_euler_accel_limit(const Vector3f_t* euler_rad, const Vector3f_t* euler_accel);

// get throttle passed into attitude controller (i.e. throttle_in provided to set_throttle_out)
static inline float attctrl_get_throttle_in(Attitude_ctrl * att_ctrl) { return att_ctrl->_throttle_in; }

// get the roll acceleration limit in centidegrees/s/s or radians/s/s
static inline float attctrl_get_accel_roll_max(Attitude_ctrl * att_ctrl) { return att_ctrl->_accel_roll_max; }
static inline float attctrl_get_accel_roll_max_radss(Attitude_ctrl * att_ctrl) { return radians(att_ctrl->_accel_roll_max * 0.01f); }

// Sets the roll acceleration limit in centidegrees/s/s
static inline void attctrl_set_accel_roll_max(Attitude_ctrl * att_ctrl, float accel_roll_max) { att_ctrl->_accel_roll_max = accel_roll_max; }

// Sets and saves the roll acceleration limit in centidegrees/s/s
static inline void attctrl_save_accel_roll_max(Attitude_ctrl * att_ctrl, float accel_roll_max) { att_ctrl->_accel_roll_max = accel_roll_max; PARAM_SET_FLOAT(ATT_CTRL,ATC_ACCEL_R_MAX,accel_roll_max);}

// get the pitch acceleration limit in centidegrees/s/s or radians/s/s
static inline float attctrl_get_accel_pitch_max(Attitude_ctrl * att_ctrl) { return att_ctrl->_accel_pitch_max; }
static inline float attctrl_get_accel_pitch_max_radss(Attitude_ctrl * att_ctrl) { return radians(att_ctrl->_accel_pitch_max * 0.01f); }

// Sets the pitch acceleration limit in centidegrees/s/s
static inline void attctrl_set_accel_pitch_max(Attitude_ctrl * att_ctrl, float accel_pitch_max) { att_ctrl->_accel_pitch_max = accel_pitch_max; }

// Sets and saves the pitch acceleration limit in centidegrees/s/s
static inline void attctrl_save_accel_pitch_max(Attitude_ctrl * att_ctrl, float accel_pitch_max) { att_ctrl->_accel_pitch_max = accel_pitch_max; PARAM_SET_FLOAT(ATT_CTRL,ATC_ACCEL_P_MAX,accel_pitch_max);}

// get the yaw acceleration limit in centidegrees/s/s or radians/s/s
static inline float attctrl_get_accel_yaw_max(Attitude_ctrl * att_ctrl) { return att_ctrl->_accel_yaw_max; }
static inline float attctrl_get_accel_yaw_max_radss(Attitude_ctrl * att_ctrl) { return radians(att_ctrl->_accel_yaw_max * 0.01f); }

// Sets the yaw acceleration limit in centidegrees/s/s
static inline void attctrl_set_accel_yaw_max(Attitude_ctrl * att_ctrl, float accel_yaw_max) { att_ctrl->_accel_yaw_max = accel_yaw_max; }

// Sets and saves the yaw acceleration limit in centidegrees/s/s
static inline void attctrl_save_accel_yaw_max(Attitude_ctrl * att_ctrl, float accel_yaw_max) { att_ctrl->_accel_yaw_max = accel_yaw_max; PARAM_SET_FLOAT(ATT_CTRL,ATC_ACCEL_Y_MAX,accel_yaw_max);}

// Return body-frame feed forward setting
static inline bool attctrl_get_bf_feedforward(Attitude_ctrl *att_ctrl) { return att_ctrl->_rate_bf_ff_enabled; }

// Return 321-intrinsic euler angles in centidegrees representing the rotation from NED earth frame to the
// attitude controller's target attitude.
// **NOTE** Using vector3f*deg(100) is more efficient than deg(vector3f)*100 or deg(vector3d*100) because it gives the
// same result with the fewest multiplications. Even though it may look like a bug, it is intentional. See issue 4895.
static inline Vector3f_t attctrl_get_att_target_euler_cd(Attitude_ctrl * att_ctrl) { return vec3_mult2(&att_ctrl->_euler_angle_target, degrees(100.0f)); }
static inline Vector3f_t attctrl_get_att_target_euler_rad(Attitude_ctrl * att_ctrl) { return att_ctrl->_euler_angle_target; }

// Return the angle between the target thrust vector and the current thrust vector.
static inline float attctrl_get_att_error_angle_deg(Attitude_ctrl * att_ctrl) { return degrees(att_ctrl->_thrust_error_angle); }

// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using smoothing_gain
float attctrl_input_shaping_angle2(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt);
static inline float attctrl_input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt){ return attctrl_input_shaping_angle2(error_angle,  smoothing_gain,  accel_max,  target_ang_vel,  0.0f,  0.0f,  dt); }

// limits the acceleration and deceleration of a velocity request
float attctrl_input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt);

// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void attctrl_euler_rate_to_ang_vel(const Vector3f_t* euler_rad, const Vector3f_t* euler_rate_rads, Vector3f_t* ang_vel_rads);

// limits angular velocity
void attctrl_ang_vel_limit(Vector3f_t* euler_rad, float ang_vel_roll_max, float ang_vel_pitch_max, float ang_vel_yaw_max);

// Convert an angular velocity vector to a 321-intrinsic euler angle derivative
// Returns false if the vehicle is pitched 90 degrees up or down
bool attctrl_ang_vel_to_euler_rate(const Vector3f_t* euler_rad, const Vector3f_t* ang_vel_rads, Vector3f_t* euler_rate_rads);

// Return the yaw slew rate limit in radians/s
static inline float attctrl_get_slew_yaw_rads(Attitude_ctrl * att_ctrl) { return radians(att_ctrl->_slew_yaw * 0.01f); }

// Sets yaw target to vehicle heading and sets yaw rate to zero
// If reset_rate is false rates are not reset to allow the rate controllers to run
void attctrl_reset_yaw_target_and_rate(Attitude_ctrl * att_ctrl, bool reset_rate);

// get the roll angular velocity limit in radians/s
static inline float attctrl_get_ang_vel_roll_max_rads(Attitude_ctrl * att_ctrl) { return radians(att_ctrl->_ang_vel_roll_max); }

// get the pitch angular velocity limit in radians/s
static inline float attctrl_get_ang_vel_pitch_max_rads(Attitude_ctrl * att_ctrl) { return radians(att_ctrl->_ang_vel_pitch_max); }

// get the yaw slew limit
static inline float attctrl_get_slew_yaw_cds(Attitude_ctrl * att_ctrl) { return att_ctrl->_slew_yaw; }

// get the rate control input smoothing time constant
static inline float attctrl_get_input_tc(Attitude_ctrl * att_ctrl) { return att_ctrl->_input_tc; }

// set the rate control input smoothing time constant
static inline void attctrl_set_input_tc(Attitude_ctrl * att_ctrl, float input_tc) { att_ctrl->_input_tc = math_constrain_float(input_tc, 0.0f, 1.0f); }

// Return tilt angle limit for pilot input that prioritises altitude hold over lean angle
static inline float attctrl_get_althold_lean_angle_max(Attitude_ctrl * att_ctrl)
{
    // convert to centi-degrees for public interface
    return MAX(ToDeg(att_ctrl->_althold_lean_angle_max), AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN) * 100.0f;
}

// Return configured tilt angle limit in centidegrees
static inline float attctrl_lean_angle_max(Attitude_ctrl * att_ctrl) { return att_ctrl->_angle_max; }

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



